package Utils;

import lejos.nxt.TouchSensor;
import lejos.nxt.UltrasonicSensor;
import lejos.robotics.navigation.DifferentialPilot;

public class MovementMethod {

	public static void localizeInStartingZone (DifferentialPilot mRobot, UltrasonicSensor mUltrasonicSensor, TouchSensor mLeftTouchSensor, TouchSensor mRightTouchSensor, int angleInterval ) throws InterruptedException {
		
		// Rotate robot to minimum distance angle
		mRobot.rotate(UltrasonicSensorMethod.getClosestWallAngle(angleInterval, mUltrasonicSensor));
		
		// Align the robot with the wall
		TouchSensorMethod.alignWithTheWall(mRobot, mLeftTouchSensor, mRightTouchSensor);
		
		// Turn right
		MovementMethod.turnRight(mRobot);
		
		// Align the robot with the wall
		TouchSensorMethod.alignWithTheWall(mRobot, mLeftTouchSensor, mRightTouchSensor);
		
		// Turn right
		MovementMethod.turnRight(mRobot);
	}
	
	public static void turnRight ( DifferentialPilot mRobot ) throws InterruptedException {
		mRobot.rotate(90);
		mRobot.stop();
		Thread.sleep(50);
	}
	
	public static void turnLeft ( DifferentialPilot mRobot ) throws InterruptedException {
		mRobot.rotate(-90);
		mRobot.stop();
		Thread.sleep(50);
	}
	
	public static void turnAround ( DifferentialPilot mRobot ) throws InterruptedException {
		mRobot.rotate(180);
		mRobot.stop();
		Thread.sleep(50);
	}
	
	public static void moveOneCellForward ( DifferentialPilot mRobot ) throws InterruptedException {
		mRobot.travel(30);
		mRobot.stop();
		Thread.sleep(50);
	}
	
	public static void moveOneCellBackward ( DifferentialPilot mRobot ) throws InterruptedException {
		mRobot.travel(-30);
		mRobot.stop();
		Thread.sleep(50);
	}
	
	public static int changeDirectionTurnRight ( int currentDirection ) {
		
		int newDirection = currentDirection + 1;
		if (newDirection == 5) {
			newDirection = 1;
		}
		return newDirection;
	}
	
	public static int changeDirectionTurnLeft ( int currentDirection ) {
		int newDirection = currentDirection - 1;
		if (newDirection == 0) {
			newDirection = 4;
		}
		return newDirection;
	}
	
	public static int changeDirectionTurnAround ( int currentDirection ) {
		
		int newDirection = currentDirection;
		
		if (currentDirection == 1) {
			newDirection = 3;
		} else if (currentDirection == 2) {
			newDirection = 4;
		} else if (currentDirection == 3) {
			newDirection = 1;
		} else if (currentDirection == 4) {
			newDirection = 2;
		}
		return newDirection;
	}
	
	public static void updateCoordinatesWithDirectionForwards ( Coordinates myCoord, int direction ) {
		
		if (direction == 1) {
			myCoord.yCoord++;
		} else if (direction == 2) {
			myCoord.xCoord++;
		} else if (direction == 3) {
			myCoord.yCoord--;
		} else if (direction == 4) {
			myCoord.xCoord--;
		}
	}
	
	public static void updateCoordinatesWithDirectionBackwards ( Coordinates myCoord, int direction ) {
		
		if (direction == 1) {
			myCoord.yCoord--;
		} else if (direction == 2) {
			myCoord.xCoord--;
		} else if (direction == 3) {
			myCoord.yCoord++;
		} else if (direction == 4) {
			myCoord.xCoord++;
		}
	}
}
